COMMENT ⊗   VALID 00003 PAGES 
RECORD PAGE   DESCRIPTION
 00001 00001
 00002 00002	DEFINE S1="16.24",S2="6.05",S6="9.38"
 00005 00003	SIMPLE PROCEDURE ARM_SOLVE(SAFE REAL ARRAY T,JREFERENCE INTEGER FLAG)
 00008 ENDMK
⊗;
DEFINE S1="16.24",S2="6.05",S6="9.38";
DEFINE S3LL="6.5",SX="20.035",SY="10.305";

	PRELOAD_WITH SX,SY,S1,1.0;
	INTERNAL SAFE REAL ARRAY SHOLDER[1:4];

	PRELOAD_WITH -185.0, 60.0, -185.0, -60.0, 6.5, 27.5, -175.0, 140.0, -100.0, 100.0;
	INTERNAL SAFE REAL ARRAY STOP[1:5,1:2];

DEFINE M6="112.3", Z6="-3.09", I6XX="1411.2", I6YY="1320.6", I6ZZ="131.3";

PRELOAD_WITH 800.0, 320.0, 76.0, 80.0, 230.0, 120.0;
SAFE OWN REAL ARRAY F0[1:6];
PRELOAD_WITH AVT(0.0, S1, 0.0, -1.0),
 	AVT(0.0, S2, 0.0, 1.0), 
	AVS(0.0, 1.0, 0.0, 0.0, -1.0),
	AVT(0.0, 0.0, 0.0, -1.0), 
	AVT(0.0, 0.0, 0.0, 1.0), 
	AVT(0.0, S6, 1.0, 0.0);
SAFE REAL ARRAY A[0:101];

PRELOAD_WITH AVP(0.0, S1, 0.0, -1.0),
 	AVP(0.0, S2, 0.0, 1.0), 
	AVP(0.0, 0.0, 0.0, 0.0, ),
	AVP(0.0, 0.0, 0.0, -1.0), 
	AVP(0.0, 0.0, 0.0, 1.0), 
	AVP(0.0, S6, 1.0, 0.0);
SAFE REAL ARRAY APAR [0:35];

PRELOAD_WITH QT, QS;
SAFE REAL ARRAY Q[0:33];

PRELOAD_WITH JDEF(1849.0, 0.0, 0.0, -1.69, 0.0, 19780.0, 0.0),
	JDEF(935.2, 0.0, -4.28, -1.67, 34678.5, 7365.2, 27907.9),
	JDEF(668.2, 0.0, 0.0, -18.1, 255446.0, 255446.0, 126.0),
	JDEF(365.0, 0.0, 1.323, 0.0, 2280.0, 628.0, 1820.0),
	JDEF(145.0, 0.0, 0.0, 2.82, 1542.0, 1450.0, 136.0),
	JDEF(112.3, 0.0, 0.0, -3.09, 1411.2, 1320.6, 131.3);
SAFE REAL ARRAY JMAT[0:101];

	SAFE REAL ARRAY MA[1:6];

	PRELOAD_WITH 0.5,0.5,4.5,0.2,0.2,0.4;
	SAFE REAL ARRAY TIMFAC[1:6];

SIMPLE PROCEDURE ARM_SOLVE(SAFE REAL ARRAY T,J;REFERENCE INTEGER FLAG);
BEGIN	SAFE OWN REAL ARRAY VT,P,W,A,Y3,Z3,Y6,Z6X[1:4];
	REAL R,TH,SN1,SN2,C1,C2;
LABEL NONE;
	INTEGER I;
BOOLEAN SIMPLE PROCEDURE JOINT4(SAFE REAL ARRAY J);
BEGIN	J[4]←ANGLE(VT,Y3,Z3);
	IF (STOP[4,1]-J[4])*(J[4]-STOP[4,2]) ≥ 0.0 THEN RETURN (TRUE);
	J[4]←IF J[4]≤ 0.0 THEN J[4]+360.0 ELSE J[4]-360.0;
	RETURN((STOP[4,1]-J[4])*(J[4]-STOP[4,2]) ≥ 0.0 );
END;

BOOLEAN SIMPLE PROCEDURE JOINT5(SAFE REAL ARRAY J);
BEGIN	J[5]←ANGLE(Z6X,Z3,VT);
	RETURN((STOP[5,1]-J[5])*(J[5]-STOP[5,2]) ≥ 0.0 );
END;

SIMPLE PROCEDURE JOINT6(SAFE REAL ARRAY J);
BEGIN	REAL R;
	R←ANGLE(Y6,VT,Z6X);
	WHILE R-J[6]>180.0 DO R←R-360.0;
	WHILE R-J[6]<-180.0 DO R←R+360.0;
	J[6]←R;
END;

	CVV(P,T,4);
	CVV(Z6X,T,3);
	SCALE(VT,Z6X,-S6);
	PLUS(W,P,VT);
	REDUCE (W);
	DIFFERENCE(A,W,SHOLDER);
	REDUCE(A);
	R←DOT(A,A);
	R←R-S2↑2;
	IF R<0.0 THEN GO TO NONE;
	J[3]←SQRT(R);
	IF (STOP[3,1]-J[3])*(J[3]-STOP[3,2])< 0.0  THEN GO TO NONE;
	TH←ATAN2((SHOLDER[2]-W[2]),(SHOLDER[1]-W[1]));
	TH←IF TH>0.0 THEN TH-TPI ELSE TH;
	R←SQRT((A[1]↑2+A[2]↑2));
	IF (R←S2/R)>1.0 THEN GO TO NONE;
	J[1]←RAD*(TH+ASIN(R));
	IF (STOP[1,1]-J[1])*(J[1]-STOP[1,2])< 0.0  THEN GO TO NONE;
	J[2]←RAD*(-ACOS(((W[3]-SHOLDER[3])/J[3])));
	SN1←SIND(J[1]);
	C1←COSD(J[1]);
	SN2←SIND(J[2]);
	C2←COSD(J[2]);
	Y3[4]←Z3[4]←1.0;
	Y3[1]←C1*C2;
	Y3[2]←SN1*C2;
	Y3[3]←-SN2;
	Z3[1]←C1*SN2;
	Z3[2]←SN1*SN2;
	Z3[3]←C2;
	CVV(Y6,T,2);
	CROSS(VT,Z3,Z6X);
	IF MAGNITUDE(VT)<1.0@-6 THEN MOVEV(VT,Y3) ELSE UNIT(VT,VT);
	IF JOINT4(J)
	THEN	BEGIN
		IF FLAG←JOINT5(J)
		THEN JOINT6(J);
		RETURN END;
	SCALE(VT,VT,-1.0);
	IF FLAG←JOINT4(J)
	THEN	IF FLAG←JOINT5(J)
		THEN	JOINT6(J);
	RETURN;
NONE:	FLAG←0;
END;